﻿#include "camera.h"

#include <QMessageBox>
#include <QFileDialog>
#include <QDesktopWidget>
#include <QtSerialPort/QSerialPortInfo>
#include <QTimer>
#include <QDebug>


#include "chess_board_distance.h"
#include "image_model_distance.h"
#include "projection_correct.h"

using cv::Mat;

CyDevice* cameraDevice;
ImageProcess* imageProcess;

Camera::Camera(QWidget *parent)
    : QWidget(parent),
    isOpen(false),
    isColor(false),
    isCapture(false),
    capturedCount(0),
    isCorrect(false),
    isCalcDist(false),
    isCross(false),
    poseCorrect(nullptr),
    autoFocus(nullptr)
{
    ui.setupUi(this);

    initWindows();
    initConfig();
    initCamera();
    initProcess();
    initCalibration();
    initCalcDistance();
    initAutoFocus();

    frames = 0;
    connect(&timer, &QTimer::timeout, [&]() {
        setWindowTitle(QStringLiteral("单目相机测距系统 - ") + QString::number(frames) + "fps");
        frames = 0;
	});
    timer.start(1000);
}

Camera::~Camera()
{
    delete poseCorrect;

    processThread.quit();
    processThread.wait();

    focusThread.quit();
    focusThread.wait();
}

void Camera::closeEvent(QCloseEvent* event)
{
    if (isOpen) {
        closeCamera();
    }
}


void Camera::initWindows()
{
    QRect desktopRect = QApplication::desktop()->availableGeometry();
    desktopRect.setHeight(1050);
    desktopRect.setWidth(1920);
    desktopRect.setHeight(desktopRect.height() - 40);
    this->setFixedSize(desktopRect.width(), desktopRect.height());

    qDebug() << "avaliable size = " << desktopRect;

    const int configWindowWdith = 320;
    const int displayWindowWidth = desktopRect.width() - configWindowWdith - 20;
    const int displayWindowHeight = desktopRect.height() - 20;

    QPixmap initPixmap(displayWindowWidth, displayWindowHeight);
    initPixmap.fill(Qt::gray);
    ui.display->setPixmap(initPixmap);
    ui.display_frame->setFixedSize({displayWindowWidth, displayWindowHeight});
}

void Camera::initConfig()
{
    const QString configName = "config/camera_config.json";
    cameraConfig = CameraConfig::Parser::readConfig(configName);

    for (auto& size : cameraConfig.resolution) {
        ui.resolution->addItem(QString::number(size.width()) + QStringLiteral("×") + QString::number(size.height()));
    }
    ui.resolution->setCurrentIndex(3);
    for (auto& bw : cameraConfig.bitwidth) {
        ui.bitwidth->addItem(QString::number(bw));
    }
    ui.bitwidth->setCurrentIndex(0);
    ui.color->addItems(cameraConfig.color);
    ui.color->setCurrentIndex(0);
    for (auto& a : cameraConfig.analog) {
        ui.analog->addItem(QString::number(a));
    }
    ui.analog->setCurrentIndex(0);
    for (auto& d : cameraConfig.digital) {
        ui.digital->addItem(QString::number(d));
    }
    ui.digital->setCurrentIndex(0);
    ui.exposure_type->addItems(cameraConfig.exposure);
    ui.exposure_type->setCurrentIndex(0);

    const QString commandName = "config/camera_command.json";
    cameraCommand = CameraConfig::Parser::readCommand(commandName);

    calibrationConfig.readConfigFrom("config/calibration_config.xml");
    ui.chessboard_wdith->setText(QString::number(calibrationConfig.boardSize.width));
    ui.chessboard_height->setText(QString::number(calibrationConfig.boardSize.height));
    ui.chessboard_size->setText(QString::number(calibrationConfig.boardSquareSize));

    distanceConfig.readFrom("config/distance_config.xml");
    ui.focal_length->setText(QString::number(distanceConfig.focalLength));
    ui.cell_size->setText(QString::number(distanceConfig.cellSize.width));
    ui.object_radius->setText(QString::number(distanceConfig.radius));
    ui.object_chessboard_size->setText(QString::number(distanceConfig.squareLength));
    ui.reference_range->setText(QString::number(distanceConfig.reference, 'g', 8));
    ui.object_scale->setText(QString::number(distanceConfig.scale, 'g', 8));
}

void Camera::initCamera()
{
    cameraDevice = new CyDevice;

    cameraDevice->setCommandSet(cameraCommand);
    cameraDevice->moveToThread(&cameraThread);

    // 分辨率改变时, 发送信号改变接收
    connect(this, &Camera::setResolution, cameraDevice, &CyDevice::setResolution);
    connect(ui.resolution, static_cast<void (QComboBox::*)(int)>(&QComboBox::currentIndexChanged), this, &Camera::changeResolution);

    connect(ui.bitwidth, static_cast<void (QComboBox::*)(const QString&)>(&QComboBox::currentIndexChanged), [&](const QString& text) {
        if (isOpen) {
            cameraDevice->setBitwidth(text.toInt());
        }
	});

    connect(ui.color, static_cast<void (QComboBox::*)(const QString&)>(&QComboBox::currentIndexChanged), [&](const QString& text) {
        isColor = text == "rgb";
	});

    connect(ui.analog, static_cast<void (QComboBox::*)(const QString&)>(&QComboBox::currentIndexChanged), [&](const QString& text) {
        if (isOpen) {
            cameraDevice->setAnalog(text.toInt());
        }
	});

    connect(ui.digital, static_cast<void (QComboBox::*)(const QString&)>(&QComboBox::currentIndexChanged), [&](const QString&) {});

    connect(ui.exposure_slider, &QSlider::valueChanged, [&](int value) {
        if (isOpen && ui.exposure_type->currentText() == "manual") {
            cameraDevice->setExposure(value);
        }
	});

    connect(ui.open_camera, &QPushButton::clicked, this, &Camera::openCamera);
    connect(ui.close_camera, &QPushButton::clicked, this, &Camera::closeCamera);
}

void Camera::initProcess()
{
    imageProcess = new ImageProcess(nullptr);

    imageProcess->moveToThread(&processThread);
    processThread.start();

    connect(cameraDevice, &CyDevice::received, imageProcess, &ImageProcess::receive);
    imageProcess->registerReceive(this, static_cast<void (QObject::*)()>(&Camera::receive));
}

void Camera::initCalibration()
{
    calibration.initBoard("config/calibration_config.xml");

    // 暂无
	/*
    connect(ui.capture, &QPushButton::clicked, [&]() {
        isCapture = true;
        ui.captureCount->setText(QString::number(++capturedCount));
	});

    connect(ui.clearCapture, &QPushButton::clicked, [=]() {
        capturedCount = 0;
        ui.captureCount->setText("0");
	});
	*/

    connect(&calibration, &Calibration::processed, this, &Camera::calibrationFinished);

    connect(ui.run_calibration, &QPushButton::clicked, [&]() {
        QString dirPath = QFileDialog::getExistingDirectory(this, ".");
        if (dirPath.isEmpty()) {
            return;
        }
        QDir dir(dirPath);

        QStringList imageNameList = dir.entryList(QDir::Files);

        calibImages.clear();
        for (auto& name : imageNameList) {
            calibImages.push_back(cv::imread((dirPath + "/" + name).toStdString()));
        }

        calibration.setImageList(&calibImages);
        calibration.start();
	});

    connect(ui.load_calib_config, &QPushButton::clicked, [&]() {
        QString filePath = QFileDialog::getOpenFileName(this, "Open calibration result file", ".", "XML files (*.xml);;All files (*.*)");
        if (filePath.isEmpty()) {
            return;
        }
        calibration.load(filePath);
	});

    connect(ui.distortion_correct, &QCheckBox::stateChanged, [&](int state) {
        if (state == Qt::Unchecked) {
            isCorrect = false;
            return;
        }

        if (calibration.cameraMatrix.empty() || calibration.distCoeffs.empty()) {
            QMessageBox::critical(this, "error", "Empty calibration parameter");
            ui.distortion_correct->setCheckState(Qt::Unchecked);
            return;
        }

        isCorrect = true;
	});
}

void Camera::calibrationFinished()
{
    QMessageBox::information(this, "information", "Calibration camera success");
    calibration.save("config/calibration_result.xml");
}

void Camera::initCalcDistance()
{
    distanceMethods[0] = new ChessBoardDistance;
    distanceMethods[1] = new ImageModelDistance;
	
    calcDistance.setCameraParameter("config/distance_config.xml", "config/calibration.xml");
    poseCorrect = new ProjectionCorrect;

    connect(ui.calculate_distance, &QPushButton::clicked, [&]() {
        ui.distance_result->setText("0");
        isCalcDist = true;
	});

    connect(ui.projective_correct, &QCheckBox::stateChanged, [&](int state) {
        calcDistance.setPoseCorrect(state == Qt::Unchecked ? nullptr : poseCorrect);
	});

    // 暂无
    /*
    connect(ui.cross, &QCheckBox::stateChanged, [&](int state) {
        isCross = state != Qt::Unchecked;
	});
    */

    connect(&calcDistance, &CalcDistance::processed, this, &Camera::calcDistanceFinished);
}

void Camera::initAutoFocus()
{
    autoFocus.moveToThread(&focusThread);
    focusThread.start();

    auto checkRegister = [&](bool checked) {
        if (checked) {
			imageProcess->registerReceive(&autoFocus, static_cast<void (QObject::*)()>(&AutoFocus::process));
            return;
        }
        imageProcess->disregisterReceive(&autoFocus);
    };

    checkRegister(ui.auto_focus->isChecked());

    for (auto& info : QSerialPortInfo::availablePorts()) {
        ui.serialport->addItem(info.portName() + ": " + info.description());
    }

    // autoFocus.connectSerialPort(ui.serialBox->currentText().split(":")[0]);
    connect(ui.connect_serial, &QPushButton::clicked, [&]() {
        QString portName = ui.serialport->currentText().split(":")[0];
        autoFocus.connectSerialPort(portName);
        qDebug() << "auto focus lens system init finished";
	});

    connect(ui.auto_focus, &QCheckBox::toggled, checkRegister);
}

void Camera::calcDistanceFinished(double length)
{
    if (length < 1e-5) {
        ui.distance_result->setText("failed");
        return;
    }
    ui.distance_result->setText(QString::number(length, 'g', 8));
    // cv::imshow("distance result", calcDistance.resultImage);
    // cv::waitKey(1);
    static int times = 0;
    qDebug() << ++times << " >> " << length;
}

void Camera::openCamera()
{
    if (isOpen) {
        return;
    }

    cameraThread.start(QThread::HighestPriority);
    isOpen = cameraDevice->open(HANDLE(this->winId()));
    if (!isOpen) {
        QMessageBox::information(this, "information", "Open camera device error");
        return;
    }
    cameraDevice->setBitwidth(ui.bitwidth->currentText().toInt());
    cameraDevice->setAnalog(ui.analog->currentText().toInt());
    emit setResolution(cameraConfig.resolution[ui.resolution->currentIndex()].width());
}

void Camera::closeCamera()
{
    if (cameraThread.isRunning()) {
        cameraDevice->enableReceive(false);

        cameraThread.quit();
        cameraThread.wait();

        cameraDevice->close();
    }
    isOpen = false;
}

void Camera::changeResolution(int index)
{
    if (!isOpen) {
        return;
    }

    if (cameraDevice->receiving()) {
        cameraDevice->enableReceive(false);
        emit setResolution(cameraConfig.resolution[index].width());
    }
    cameraDevice->wait();
    cameraDevice->setBitwidth(ui.bitwidth->currentText().toInt());
    cameraDevice->setAnalog(ui.analog->currentText().toInt());
}

void Camera::receive()
{
    if (!isOpen || imageProcess->empty(this)) {
        return;
    }
    ++frames;

    Mat image = imageProcess->read(this);
    if (image.empty()) {
        return;
    }

    if (!isColor) {
        cv::cvtColor(image, image, cv::COLOR_BGR2GRAY);
    }

    if (isCorrect) {
        calibration.correct(image, image);
    }

    if (isCapture) {
        isCapture = false;
        cv::imwrite("captured/" + std::to_string(capturedCount) + ".png", image);
    }

    if (ui.projective_correct->checkState() == Qt::Checked) {
        poseCorrect->correct(image, image);
    }

    if (isCalcDist) {
        isCalcDist = false;
        calcDistance.setImage(image);
        calcDistance.setDistance(distanceMethods[ui.distance_method->currentIndex()]);
        calcDistance.start();
    }

    if (isCross) {
        if (image.type() != CV_8UC3) {
            cv::cvtColor(image, image, cv::COLOR_GRAY2BGR);
        }
        int rows = image.rows, cols = image.cols;
        cv::line(image, { 0, rows / 2 }, { cols - 1, rows / 2 }, cv::Scalar(0, 0, 255));
        cv::line(image, { cols / 2, 0 }, { cols / 2, rows - 1 }, cv::Scalar(0, 0, 255));
    }

    imshow(image);
}

void Camera::imshow(cv::Mat& image)
{
	int width = ui.display->pixmap()->size().width(), height = ui.display->pixmap()->size().height();
    cv::Scalar value = { 0xA0, 0xA0, 0xA0 };
    if (image.type() == CV_8UC1) {
        value = { 0xA0 };
    }
    cv::Mat background = cv::Mat(height, width, image.type(), value);

	int newrow = image.rows, newcol = image.cols;
	if (newrow > height && newcol > width) {
		double ratio;
		if (static_cast<double>(newrow) / newcol > static_cast<double>(height) / width) {
			ratio = static_cast<double>(newrow) / height;
		}
		else {
			ratio = static_cast<double>(newcol) / width;
		}
		newrow = newrow / ratio;
		newcol = newcol / ratio;
	} else if (newrow > height || newcol > width) {
		double ratio = std::max(static_cast<double>(newrow) / height, static_cast<double>(newcol) / width);
		newrow = newrow / ratio;
		newcol = newcol / ratio;
	}

	cv::resize(image, image, { newcol, newrow });
	image.copyTo(background(cv::Rect((width - newcol)/2, (height - newrow)/2, newcol, newrow)));

    QPixmap pixmap;
    if (image.type() == CV_8UC3) {
        pixmap = QPixmap::fromImage(QImage(background.data, background.cols, background.rows, QImage::Format_BGR888));
    }
    else {
        pixmap = QPixmap::fromImage(QImage(background.data, background.cols, background.rows, QImage::Format_Grayscale8));
    }
	pixmap.scaled(ui.display->pixmap()->size(), Qt::KeepAspectRatio, Qt::SmoothTransformation);
	ui.display->setPixmap(pixmap);
}
